/*
===========================================================================
This source file is part of DyLab (Dynamic Laboratory)
For the latest info, see http://dylab.googlecode.com

Copyright (c) 2006-2008 Lukas Krejci
(krejci.lukas@volny.cz)

This file is part of DyLab.

    DyLab is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    DyLab is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with DyLab.  If not, see <http://www.gnu.org/licenses/>.
===========================================================================
*/

//===========================================================================================
#include <dylab/utils/geometry/TetrahedronGeometry.h>
#include <dylab/utils/math/Matrix3x3.h>

//===========================================================================================
void dylab::TetrahedronGeometry::getEdgeVertexIndices(edgeidx_t _edgeIdx, 
	vertexidx_t (&_vertexIndices)[EdgeGeometry3::V_COUNT])
{
    switch (_edgeIdx)
    {
    case E_0: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_0,
        _vertexIndices[EdgeGeometry3::V_1] =  V_1; 
        break;
    case E_1: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_0,
        _vertexIndices[EdgeGeometry3::V_1] =  V_2; 
        break;
    case E_2: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_0,
        _vertexIndices[EdgeGeometry3::V_1] =  V_3; 
        break;
    case E_3: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_1,
        _vertexIndices[EdgeGeometry3::V_1] =  V_2; 
        break;
    case E_4: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_1,
        _vertexIndices[EdgeGeometry3::V_1] =  V_3; 
        break;
    case E_5: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_2,
        _vertexIndices[EdgeGeometry3::V_1] =  V_3; 
        break;
    default: 
        throw EInvalidArgument(String::fmt("Invalid edge index: %1%", static_cast<uint32_t>(_edgeIdx)));
    }
}
//-----------------------------------------------------------------------------
void dylab::TetrahedronGeometry::getFaceVertexIndices(faceidx_t _faceIdx, 
	vertexidx_t (&_vertexIndices)[TriangleGeometry3::V_COUNT])
{ 
    switch (_faceIdx)
    {
    case F_0:
        _vertexIndices[TriangleGeometry3::V_0] = V_1, 
        _vertexIndices[TriangleGeometry3::V_1] = V_2, 
        _vertexIndices[TriangleGeometry3::V_2] = V_3;
        break;
    case F_1: 
        _vertexIndices[TriangleGeometry3::V_0] = V_2, 
        _vertexIndices[TriangleGeometry3::V_1] = V_0, 
        _vertexIndices[TriangleGeometry3::V_2] = V_3;
        break;
    case F_2:
        _vertexIndices[TriangleGeometry3::V_0] = V_3, 
        _vertexIndices[TriangleGeometry3::V_1] = V_0, 
        _vertexIndices[TriangleGeometry3::V_2] = V_1;
        break;
    case F_3:
        _vertexIndices[TriangleGeometry3::V_0] = V_0, 
        _vertexIndices[TriangleGeometry3::V_1] = V_2, 
        _vertexIndices[TriangleGeometry3::V_2] = V_1;
        break;
    default: 
        throw EInvalidArgument(String::fmt("Invalid face index: %1%", static_cast<uint32_t>(_faceIdx)));
    }
}
//-----------------------------------------------------------------------------

//===========================================================================================
dylab::Vector4 dylab::TetrahedronGeometry::computeBarycentricCoords(const Vector3 & _p) const
{
	real_t a[3][4];     // 3x4 linear system matrix

	// set-up the 3x4 linear system matrix
	a[0][0] = mVertices[V_1].x() - mVertices[V_0].x();
	a[0][1] = mVertices[V_2].x() - mVertices[V_0].x();
	a[0][2] = mVertices[V_3].x() - mVertices[V_0].x();
	a[0][3] = _p.x() - mVertices[V_0].x();

	a[1][0] = mVertices[V_1].y() - mVertices[V_0].y();
	a[1][1] = mVertices[V_2].y() - mVertices[V_0].y();
	a[1][2] = mVertices[V_3].y() - mVertices[V_0].y();
	a[1][3] = _p.y() - mVertices[V_0].y();

	a[2][0] = mVertices[V_1].z() - mVertices[V_0].z();
	a[2][1] = mVertices[V_2].z() - mVertices[V_0].z();
	a[2][2] = mVertices[V_3].z() - mVertices[V_0].z();
	a[2][3] = _p.z() - mVertices[V_0].z();

	// solve the 3x4 linear system
	real_t det  = Matrix3x3(a[0][0], a[1][0], a[2][0], 
							a[0][1], a[1][1], a[2][1], 
							a[0][2], a[1][2], a[2][2]).computeDeterminant();
	real_t detx = Matrix3x3(a[0][3], a[1][3], a[2][3], 
							a[0][1], a[1][1], a[2][1], 
							a[0][2], a[1][2], a[2][2]).computeDeterminant();
	real_t dety = Matrix3x3(a[0][0], a[1][0], a[2][0], 
							a[0][3], a[1][3], a[2][3], 
							a[0][2], a[1][2], a[2][2]).computeDeterminant();
	real_t detz = Matrix3x3(a[0][0], a[1][0], a[2][0], 
							a[0][1], a[1][1], a[2][1], 
							a[0][3], a[1][3], a[2][3]).computeDeterminant();

	// determinat shouldn't be zero
	if (det == Math::ZERO)
		throw ENumericError(String::fmt("Can't compute barycentric coordinates for point %1% inside the tetrahedron %2% (degenerated: %3%, value: %4%)", _p, *this, isDegenerated(), computeValue()));

	// store the solution & compute the last barycentric coordinate
	Vector4 bcCoords;
	bcCoords.x() = detx / det;
	bcCoords.y() = dety / det;
	bcCoords.z() = detz / det;
	bcCoords.w() = Math::ONE - bcCoords.x() - bcCoords.y() - bcCoords.z();

	// return the computed barycentric coordinates
	return bcCoords;
}
//-----------------------------------------------------------------------------
dylab::TetrahedronGeometry::pointcls_t 
dylab::TetrahedronGeometry::classifyPoint(const Vector3 & _p) const
{
	// compute barycentric coordinates of the point _p inside the tetrahedron
	Vector4 bc4 = computeBarycentricCoords(_p);
	Vector3 bc3 = bc4.getVector3();

	// store the zero flag of all coords
	bool xAlmostZero = Math::almostZero(bc4.x());
	bool yAlmostZero = Math::almostZero(bc4.y());
	bool zAlmostZero = Math::almostZero(bc4.z());
	bool wAlmostZero = Math::almostZero(bc4.w());

	// first test, whether the point is not outside
	real_t bc3sum = bc3.sum();
	if ((((bc3.x() > Math::ZERO) || xAlmostZero) && ((bc3.y() > Math::ZERO) || yAlmostZero) && 
		 ((bc3.z() > Math::ZERO) || zAlmostZero)) && (bc3sum < Math::ONE || Math::almostEqual(bc3sum, Math::ONE)))
	{
		// test, if point is one of tetrahedron's vertices
		if (xAlmostZero && yAlmostZero && zAlmostZero)                                
			return pointcls_t(Math::POINTCLS_VERTEX, V_0);
		if (wAlmostZero && yAlmostZero && zAlmostZero)
			return pointcls_t(Math::POINTCLS_VERTEX, V_1);
		if (wAlmostZero && xAlmostZero && zAlmostZero)
			return pointcls_t(Math::POINTCLS_VERTEX, V_2);
		if (wAlmostZero && xAlmostZero && yAlmostZero)
			return pointcls_t(Math::POINTCLS_VERTEX, V_3);

		// test, if point lies in one of tetrahedron's edges
		if (yAlmostZero && zAlmostZero)
			return pointcls_t(Math::POINTCLS_EDGE, E_0);
		if (xAlmostZero && zAlmostZero)
			return pointcls_t(Math::POINTCLS_EDGE, E_1);
		if (xAlmostZero && yAlmostZero)
			return pointcls_t(Math::POINTCLS_EDGE, E_2);
		if (zAlmostZero && wAlmostZero)
			return pointcls_t(Math::POINTCLS_EDGE, E_3);
		if (yAlmostZero && wAlmostZero)
			return pointcls_t(Math::POINTCLS_EDGE, E_4);
		if (xAlmostZero && wAlmostZero)
			return pointcls_t(Math::POINTCLS_EDGE, E_5);

		// test, if point lies in one of tetrahedron's faces
		if (wAlmostZero)
			return pointcls_t(Math::POINTCLS_FACE, F_0);
		if (xAlmostZero)
			return pointcls_t(Math::POINTCLS_FACE, F_1);
		if (yAlmostZero)
			return pointcls_t(Math::POINTCLS_FACE, F_2);
		if (zAlmostZero)
			return pointcls_t(Math::POINTCLS_FACE, F_3);

		// _p is inner point
		return pointcls_t(Math::POINTCLS_INNER, FeatureIdx());
	}
	else
	{
		// the point is outside
		return pointcls_t(Math::POINTCLS_OUTER, FeatureIdx());
	}
}
//-----------------------------------------------------------------------------

//===========================================================================================    
std::ostream & dylab::operator << (std::ostream & _os, const TetrahedronGeometry::pointcls_t & _pointCls)
{ 
    switch (_pointCls.first)
    {
    case Math::POINTCLS_INNER: 
        return _os << "POINTCLS_INNER"; 
    case Math::POINTCLS_VERTEX: 
        return _os << String::fmt("POINTCLS_VERTEX(V_%1%)", _pointCls.second.vertexIdx); 
    case Math::POINTCLS_EDGE: 
        return _os << String::fmt("POINTCLS_EDGE(E_%1%)", _pointCls.second.edgeIdx); 
    case Math::POINTCLS_FACE: 
        return _os << String::fmt("POINTCLS_FACE(F_%1%)", _pointCls.second.faceIdx); 
    case Math::POINTCLS_OUTER: 
        return _os << "POINTCLS_OUTER"; 
    default: 
        throw EInvalidArgument(String::fmt("Invalid point-tetrahedron classification: %1%", _pointCls.first));
        return _os;
    }
}
//-----------------------------------------------------------------------------